import lejos.nxt.Sound;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.subsumption.Behavior;

//TODO: sleep aos sonars depois de detectarem parede lateral


public class SideSonars implements Behavior {

	private UltrasonicSensor rightSonar;
	private UltrasonicSensor leftSonar;
	private MapMaker mm = null;

	public SideSonars(MapMaker mm){
		this.mm = mm;
		this.rightSonar = mm.rightSonar;
		this.leftSonar = mm.leftSonar;
	}
	@Override
	public boolean takeControl()  {
		rightSonar.ping(); 
		leftSonar.ping();
		return rightSonar.getDistance() < 20 || leftSonar.getDistance() < 20; 

	}

	@Override
	public void suppress() {
		mm.snav.stop();

	}

	@Override
	public void action() {

		while (leftSonar.getDistance() < 10)
		{
			mm.snav.rotate(-10);

			if (leftSonar.getDistance() < 20)
			{
				Sound.buzz();
				break;
			}
		}


		/*if(rightSonar.getDistance() < 20) {
			mm.snav.rotate(10);

		}*/

		do	{ mm.UpdatePose(); }	while( mm.snav.isMoving() );
	}

}
